/*
 * Ken Anderson, 2009
 */

#include "Search.h"
#include <queue>
#include <vector>
#include "util.h"
#include <assert.h>

#define INITIAL_BEST_COST 99999.0

Search::Search( Graph* graph, Heuristic * heuristic )
{
	mGraph = graph;
	mHeuristic = heuristic;
	mStart = NULL;
	mGoal = NULL;
	mNumExpanded = 0;
	mMaxSizeOpenList = 0;
	mSearchRequired = false;
}

Search::~Search()
{
}

void Search::Init(Node *start, Node *goal)
{
	if( mStart != start )
	{
		mStart = start;
		mSearchRequired = true;
	}
	if( mGoal != goal)
	{
		mGoal = goal;
		mSearchRequired = true;
	}
	mHeuristic->Init( start, goal );
}


void IDAStar::Init(Node *start, Node *goal)
{
	Search::Init(start,goal);
	if( mSearchRequired)
	{
		mMaxDepth=0.0f;
		mNumExpanded = 0;
		mMaxSizeOpenList = 0;
		mBestEdge = NULL;
		mClosedList.clear();
		mMinimalPath.clear();
		mBestCost = INITIAL_BEST_COST;
	}
}

float IDAStar::RecursiveSearch(Node* current, float depth)
{
	// successfully found the goal (consider expanded)
	if( current->GetID() == mGoal->GetID() )
	{
		mClosedList[current] = ClosedListElement(depth, 1, NULL);
		return 0.0;
	}

	// went too far
	if( depth + mHeuristic->GetValue(current) >= mMaxDepth )
		return INITIAL_BEST_COST;
	
	// Mark as closed if *the node isn't closed* or *closed with incorrect value*
	if( mClosedList.find(current) == mClosedList.end() )
		mClosedList[current] = ClosedListElement(depth, 0, NULL);
	else if( mClosedList[current].mGValue > depth )
		mClosedList[current].mGValue = depth;
	mClosedList[current].mClosedCount++;	// Always increase the counter
	mNumExpanded++;

	// Expanding Node / Go through all the edges
	for(unsigned int i=0; i<current->NumOutEdges(); i++)
	{
		Edge* edge = current->GetOutEdge(i);
		float weight = edge->GetWeight();
		float cost = weight + RecursiveSearch( edge->ToNode(), depth+weight );

		// Found a solution.  Assume best solution.  Early exit.
		if( cost < INITIAL_BEST_COST )
		{
			mMinimalPath.push_back(edge);
			mSearchRequired = false;
			return cost;
		}
	}
	return INITIAL_BEST_COST;
}

// One iteration of IDA*
bool IDAStar::IterateSearch()
{
	//Early exit
	if( !mSearchRequired )
		return true;

	// Init
	float cost = INITIAL_BEST_COST;
	int i=0;
	
	// Search at increasing depths
	while(cost==INITIAL_BEST_COST && mMaxDepth < 100.0f && i<mNumNodeExpansionsOnIterate)
	{
		cost = RecursiveSearch(mStart, 0);
		mMaxDepth+=1.0;
		i++;
	}

	if( aboutEqualTo(cost,INITIAL_BEST_COST) )
		return false;
	return true;
}

// IDA*
// return cost of minimal path
// If unsuccessful, return INITIAL_BEST_COST
float IDAStar::FindMinimalPath( )
{
	//Early exit
	if( !mSearchRequired )
		return mBestCost;

	// Init
	float cost = INITIAL_BEST_COST;

	// Search at increasing depths
	for( ; cost == INITIAL_BEST_COST && mMaxDepth < 100.0f; mMaxDepth+=1.0f)
	{
		cost = RecursiveSearch(mStart, 0);
	}

	mSearchRequired = false;
	return mBestCost;
}

// expand the node (remove from open, put on closed list.
// return true if found goal
bool AStar::ExpandTopNode()
{
	if(TRACE) printf("OpenListSize=%d, ClosedListSize=%d \n", (int)mOpenList.size(), (int)mClosedList.size() );

	// Error check
	if( mOpenList.empty() ) {
		if(FATAL) printf("Error: Node not found.  Probably not reachable.\n");
		exit(0);
	}

	OpenListElement element = mOpenList.top();
	Node * node = element.mNode;
	//Edge * inEdge = element.mInEdge;
	float gValue = element.mGValue;

	// Expand node if *the node isn't closed* (or *closed with incorrect value*)
	if( mClosedList.find(node) == mClosedList.end() || mClosedList[node].mGValue > gValue )
	{
		
		// Take off open list & put on closed list.
		if(TRACE)  printf("  Expanding node=%d g=%f f=%f \n", node->GetID(), gValue, element.mFValue );
		mOpenList.pop();
		mClosedList[node].mGValue = gValue;
		mClosedList[node].mClosedCount++;
		mNumExpanded++;
		mClosedList[node].mSolutionPathEdge = element.mInEdge;
		if( mMaxSizeOpenList < mOpenList.size() ) {
			mMaxSizeOpenList = mOpenList.size();
		}

		// Add all successors to open list
		unsigned int numEdges = node->NumOutEdges();
		for( unsigned int i=0; i<numEdges; i++ )
		{
			Edge * outEdge = node->GetOutEdge(i);
			Node* node2 = outEdge->ToNode();
			float gValue2 = gValue + outEdge->GetWeight();
			OpenListElement element2 ( node2, outEdge, gValue2+mHeuristic->GetValue(node2), gValue2 );
			if(TRACE) printf("    Opening node=%d g=%f f=%f \n", node2->GetID(), gValue2, element2.mFValue );
			mOpenList.push( element2 );
		}
		// Found goal, search is complete
		if( node == mGoal )
		{
			ReconstructSolution();
			return true;
		}
	}
	else
	{
		if(TRACE) printf("  Not Expanding node=%d g=%f f=%f \n", node->GetID(), gValue, element.mFValue );
		mOpenList.pop();
	}
	return false;
}

// One iteration of A*
bool AStar::IterateSearch()
{
	// Error check
	if( mClosedList.find(mGoal)!=mClosedList.end() )
		return true;
	assert( mOpenList.size() > 0 );

	int numIterations = mNumNodeExpansionsOnIterate;
	while( mOpenList.size()>0 && (numIterations--)>0  )
	{
		if( ExpandTopNode() )
			return true;
	}

	return false;
}

void AStar::Init(Node* start, Node* goal)
{
	Search::Init(start,goal);

	if( mSearchRequired )
	{
		mNumExpanded = 0;
		mMaxSizeOpenList = 0;
		mBestCost = 0.0;
		mClosedList.clear();
		mMinimalPath.clear();
		mClosedList.clear();
		while( !mOpenList.empty() )	// Clear the open list
			mOpenList.pop();
		mOpenList.push( OpenListElement( mStart, NULL, mHeuristic->GetValue(mStart), 0.0f ) );	// Start node added to open list
		if(DEBUG) printf("goal=%d start=%d \n", mGoal->GetID(), mStart->GetID() );
	}
}

// Error if -99999.0 returned
float AStar::FindMinimalPath()
{
	//Early exit
	if( !mSearchRequired )
		return mBestCost;

	// Initialize
	mOpenList.push( OpenListElement( mStart, NULL, mHeuristic->GetValue(mStart), 0.0f ) );	// Start node added to open list
	if(TRACE) printf("goal=%d start=%d \n", mGoal->GetID(), mStart->GetID() );

	// Expand nodes on open list, one at a time.
	while( mOpenList.size()>0 && !IterateSearch() );

	mSearchRequired = false;
	return mBestCost;
}

float AStar::ResumeSearch( Node * node, bool stopWhenFound )
{
	// Search until we have expanded the node
	while( true ) {
		// if node is found, exit early and return distance
		if( stopWhenFound && IsClosed(node) )
			return mClosedList[node].mGValue;
		
		// return if search has covered the entire graph
		if( !stopWhenFound && mOpenList.empty() && IsClosed(node) ) {
			return mClosedList[node].mGValue;
		}

		// Error check (this is a duplicate, but we can only get the node ID at this point.
		if( mOpenList.empty() && !IsClosed(node) ) {
			if(FATAL) printf("Could not find nodeID %i x %f y %f.\n", node->GetID(), node->GetX(), node->GetY());
			exit(0);
		}

		// Continue search
		ExpandTopNode();
	}
	return mClosedList[node].mGValue;
}

// Reconstruct Solution Path
void AStar::ReconstructSolution()
{
	if(DEBUG) printf("Solution node %d \n", mGoal->GetID() );
	float cost = 0.0f;
	if( mClosedList.find(mGoal)==mClosedList.end() )	// early exit if solution not found
		return;
	else if( mGoal != mStart )	//skip if solution path is empty
	{
		ClosedListElement element = mClosedList[ mGoal ];
		Edge* edge = NULL;
		do
		{
			edge = element.mSolutionPathEdge;
			if( edge == NULL )
				break;

			// Otherwise add edge to minimal path
			if(DEBUG) printf("Solution node %d \n", edge->FromNode()->GetID() );
			cost += edge->GetWeight();
			mMinimalPath.push_back( edge );
			element = mClosedList[ edge->FromNode() ];
		} while( edge->FromNode() != mStart );
	}
	mBestCost = cost;
}

